import math

# ==== 已知参数 ====
FY1 = (17800.0, 0.0, 1800.0)    # 无人机 FY1 初始位置
T = (0.0, 200.0, 0.0)           # 真目标中心
M1 = (20000.0, 0.0, 2000.0)     # 导弹 M1 初始位置
v_uav = 120.0                   # UAV 速度 (m/s)
t_drop = 1.5                    # 投放时间 (s)
fuse = 3.6                      # 引信延时 (s)
g = 9.8                         # 重力加速度 (m/s^2)
cloud_r = 10.0                  # 云团半径 (m)
cloud_sink = 3.0                # 云团下沉速度 (m/s)
v_missile = 300.0               # 导弹速度 (m/s)

# ==== UAV 飞向原点的水平单位向量 ====
hx, hy = -FY1[0], -FY1[1]
h_norm = math.hypot(hx, hy)
ux, uy = hx / h_norm, hy / h_norm

# ==== 起爆时刻与位置 ====
t_explosion = t_drop + fuse
C0 = (
    FY1[0] + v_uav * ux * t_explosion,   # x
    FY1[1] + v_uav * uy * t_explosion,   # y
    FY1[2] - 0.5 * g * fuse**2           # z
)

# ==== 导弹速度向量 ====
to_origin = (-M1[0], -M1[1], -M1[2])
norm = math.sqrt(sum(c*c for c in to_origin))
v_m = tuple(c / norm * v_missile for c in to_origin)

# ==== 采样计算遮蔽区间 (Model B) ====
dt = 0.01   # 时间步长 (s)
shield_flags = []
times = [i*dt for i in range(int(20/dt)+1)]

for tau in times:
    # 导弹位置 (全局时间 t = t_explosion + tau)
    t_global = t_explosion + tau
    Mx = M1[0] + v_m[0]*t_global
    My = M1[1] + v_m[1]*t_global
    Mz = M1[2] + v_m[2]*t_global

    # 云团中心
    Cx, Cy, Cz = C0[0], C0[1], C0[2] - cloud_sink*tau

    # 判断云团球体是否与 M→T 线段相交
    vx, vy, vz = T[0]-Mx, T[1]-My, T[2]-Mz
    seg_len2 = vx*vx + vy*vy + vz*vz
    if seg_len2 == 0:
        dist2 = (Cx-Mx)**2 + (Cy-My)**2 + (Cz-Mz)**2
    else:
        wx, wy, wz = Cx-Mx, Cy-My, Cz-Mz
        u = (wx*vx + wy*vy + wz*vz) / seg_len2
        if u < 0:   # 最近点在导弹处
            px, py, pz = Mx, My, Mz
        elif u > 1: # 最近点在目标处
            px, py, pz = T
        else:       # 最近点在导弹-目标连线上
            px, py, pz = Mx + u*vx, My + u*vy, Mz + u*vz
        dist2 = (Cx-px)**2 + (Cy-py)**2 + (Cz-pz)**2

    shield_flags.append(dist2 <= cloud_r**2)

# ==== 提取连续遮蔽区间 ====
intervals, total_time = [], 0.0
i = 0
while i < len(times):
    if shield_flags[i]:
        j = i
        while j+1 < len(times) and shield_flags[j+1]:
            j += 1
        start, end = times[i], times[j]+dt
        intervals.append((start, end))
        total_time += end - start
        i = j + 1
    else:
        i += 1

# ==== 输出结果 ====
print("Explosion time:", t_explosion, "s")
print("Explosion position C0:", C0)
print("有效遮蔽区间 (s after explosion):", intervals)
print("总有效遮蔽时长:", total_time, "s")
